/**
 * @file:          Filter_FirstOrder.c
 * @brief:
 * @details:
 * @author:        wjh
 * @date created:  2023.07.11
 * @version:       1.0.0
 * @par copyright (c):
 *
 * @par history (desc):
 *   version:1.0.0, wjh, 2023.07.11,13:55:18
 */

/* Include Files **************************************************************/
#include "Filter_FirstOrder.h"
#include "common.h"

/* Global Variable Define *****************************************************/

/* Function Define ************************************************************/
void Filter_FirstOrder_Init(struct Filter_FirstOrder *self)
{
    self->Run = Filter_FirstOrder_Run;
    self->Clear = Filter_FirstOrder_Clear;
    self->UpdateParam = Filter_FirstOrder_UpdateParam;

    self->UpdateParam(self);
}

float32 Filter_FirstOrder_Run(struct Filter_FirstOrder *self, float32 in)
{
    if (self->param_en != 0)
    {
        self->in_x = in;

        self->y_last = self->out_y;
        self->out_y = (1 - self->alpha) * self->in_x + self->alpha * self->y_last;
    }
    else
    {
        self->in_x = in;
        self->out_y = self->in_x;
    }

    return self->out_y;
}

void Filter_FirstOrder_Clear(struct Filter_FirstOrder *self)
{
    self->y_last = 0;
}

void Filter_FirstOrder_UpdateParam(struct Filter_FirstOrder *self)
{
    self->Tc = 1 / (BM_2PI * self->param_fc);
    self->alpha = self->Tc / (self->Tc + self->param_Ts);
}